#include "Sys3D.h"
#include "FastOps.h"

#define nproc 4

#include <iostream>
#include <cstdio>

using namespace std;
using namespace RSIM;

int main(){
        const double& q = 1.0;
        const double& u = 1.0;
        
        const int& nbodies = 24;
                
        Sys3D mbs3D(nbodies,nproc);
        mbs3D.setQ(q);
        mbs3D.setU(u);
                
        int ids[nproc+1]=   {0,6,12,18,nbodies};
        
        // Bodies 0-5 (SubSys 1)
        mbs3D.assembleKinematicSweep(ids[0],ids[1]-1);
        
        // Bodies 6-11 (SubSys 2)
        mbs3D.assembleKinematicSweep(ids[1],ids[2]-1);
        
        // Bodies 12-17 (SubSys 3)
        mbs3D.assembleKinematicSweep(ids[2],ids[3]-1);
        
        // Bodies 18-23 (SubSys 4)
        mbs3D.assembleKinematicSweep(ids[3],ids[4]-1);
        
        // This is done serially instead of the traditional assembly-disassembly
        mbs3D.setKinematicBoundaryConditions(ids,nproc);
        
        // Just Subsystems 2,3 and 4 need to be disassembled
        // Subsystem 1 is already done
        mbs3D.disassembleKinematicSweep(ids[1],ids[2]-1);
        mbs3D.disassembleKinematicSweep(ids[2],ids[3]-1);
        mbs3D.disassembleKinematicSweep(ids[3],ids[4]-1);
        
        for(int i=0;i<nbodies-1;++i)
                cout<<mbs3D.barray_[i].w_<<endl;
        
//         mbs3D.n_formInboardEOM(ids[0],ids[1]-1);
//         mbs3D.n_formInboardEOM(ids[1],ids[2]-1);
//         mbs3D.n_formInboardEOM(ids[2],ids[3]-1);
//         mbs3D.n_formInboardEOM(ids[3],ids[4]-1);
//                 
//         
//         mbs3D.n_triangularizeBackward(0,nbodies-1);
//         mbs3D.n_substituteForward(0,nbodies-1,NULL);
        
//         mbs3D.displayUDot();
        return 0;
}